/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file threadedPipe.cxx
	\brief 

	\author Jack Elston
		$Date: 2005/02/12 00:20:45 $
*/

#include <iomanip>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>

#include "color.h"
#include "threadedPipe.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
/*<-End Global Variables-->*/

ThreadedPipe::ThreadedPipe(String name) : pThread(name), fifo_msg(buf)
{
	for(int i=0; i<4; i++) fifo_FDs[i] = -1;
	fifo_names[0] = name + " IT Fifo";
	fifo_names[1] = name + " IR Fifo";
	fifo_names[2] = name + " OT Fifo";
	fifo_names[3] = name + " OR Fifo";

	fifo_msg.src.s_addr = INADDR_ANY;
	fifo_msg.dest.s_addr = INADDR_ANY;
}

String ThreadedPipe::getFifoName(InputType type) 
{
	if(fifo_FDs[type] == -1)
		return String("");
	return fifo_names[type];
}

/************************************************************\
|                  ThreadedPipe::addFifo                     |
\************************************************************/
/*!
	\brief
	\param
	\pre
	\post
	\return
 */
bool ThreadedPipe::addFifo(InputType type, String name, FifoType_t data)
{
	//cout << "ThreadedPipe::addFifo" << endl;

	int flags = O_RDWR | O_NONBLOCK;
	if(name != ""){
		switch(type) {
			case INBOARD_RX:
			case OUTBOARD_RX:
				fifo_names[type] = name;
				flags = O_RDONLY;
				break;
			case INBOARD_TX:
			case OUTBOARD_TX:
				fifo_names[type] = name;
				break;
			default:
				break;
		}
	}
	
	if (mkfifo(fifo_names[type].c_str(), 0777) != 0 )
		if( errno != EEXIST) {
			cout << getName() << "::addFifo() - could not create " << fifo_names[type] 
				<< ", " <<  strerror(errno) << endl;
			return false;
		}

	if((fifo_FDs[type] = open(fifo_names[type].c_str(), flags )) <= 0) {
		cout << getName() << "::addFifo() - could not open " << fifo_names[type] 
      << ", " <<  strerror(errno) << endl;
		return false;
	}

	return true;
}

/************************************************************\
|                  ThreadedPipe::readFifo                    |
\************************************************************/
int ThreadedPipe::readFifo(InputType type)
{
  //cout << "ThreadedPipe::readFifo(InputType type) - type=" << (int)type << endl;
	int n = readFifo(fifo_FDs[type]);
	if(n > 0) {
		if(verbose) {
			cout << "[";
			cout << RED  << setw(16) << inet_ntoa(fifo_msg.src) << ATTROFF;
			cout << BLUE << setw(16) << inet_ntoa(fifo_msg.dest) << ATTROFF
				<< MAGENTA << setw(5) << fifo_msg.port << ATTROFF
				<< CYAN << setw(4) << fifo_msg.size << ATTROFF << "] "
				<< YELLOW << fifo_names[type] << ATTROFF << endl;
		}
	}
	else if (n == -1)
		if( errno != EAGAIN && errno != EINTR )
			cout << getName() << "::readFifo() - error reading from " << fifo_names[type] << ", " <<  strerror(errno) << endl;
	return n;
}

int ThreadedPipe::readFifo(int fd)
{
  //cout << "ThreadedPipe::readFifo(int fd) - fd=" << fd << endl;
	int n;
	n = read(fd, (char *)(&fifo_msg), 12);
	if( n != 12 ) {
		if( n == -1 && errno != EAGAIN && errno != EINTR )
			cout << getName() << "::readFifo() - error reading leading bytes from fifo " << endl;
		fifo_msg.size = 0;
		return 0;
	}

	n = 0;
	int total = 0;
	do {
		n = read(fd, (void *)(fifo_msg.data+total), fifo_msg.size);
		if( n >= 0 ) { 
			total += n;
			if( total < fifo_msg.size && verbose )
				cout << getName() << "::readFifo() - partial read from fifo " << endl;
		} else if( n < 0 && errno != EAGAIN) {
			cout << getName() << "::readFifo() - error reading from fifo: " << strerror(errno) << endl;
			return -1;
		}
		//if( total < fifo_msg.size && verbose )
			//cout << getName() << "::readFifo() - need to read more bytes " << endl;

	} while (total < fifo_msg.size);
	return total;

}

/************************************************************\
|                 ThreadedPipe::writeFifo                    |
\************************************************************/
int ThreadedPipe::writeFifo(InputType type)
{
	//cout <<"ThreadedPipe::writeFifo(InputType type)" << endl;
	int n = writeFifo(fifo_FDs[type]);
	if(n > 0) {
		if(verbose) {
			cout << "[";
			cout << RED  << setw(16) << inet_ntoa(fifo_msg.src) << ATTROFF;
			cout << BLUE << setw(16) << inet_ntoa(fifo_msg.dest) << ATTROFF
				<< MAGENTA << setw(5) << fifo_msg.port << ATTROFF
				<< CYAN << setw(4) << fifo_msg.size << ATTROFF << "] "
				<< YELLOW << fifo_names[type] << ATTROFF << endl;
		}
	}
	else if (n == -1)
		cout << getName() << "::writeFifo() - error writing to " << fifo_names[type] << ", " <<  strerror(errno) << endl;
	return n;
}

int ThreadedPipe::writeFifo(int fd)
{
	//cout <<"ThreadedPipe::writeFifo(int fd)" << endl;
	int n;
	n = write(fd, (char *)(&fifo_msg), 12);
	if( n != 12 ) {
		cout << getName() << "::writeFifo() - error writing leading bytes to fifo " << endl;
		fifo_msg.size = 0;
		return -1;
	}

	n = 0;
	int total = 0;
	do {
		n = write(fd, (void *)(fifo_msg.data+total), fifo_msg.size - total);
		if( n >= 0 ) { 
			total += n;
			if( total < fifo_msg.size && verbose )
				cout << getName() << "::writeFifo() - partial write to fifo " << endl;
		} else if( n < 0 && errno != EAGAIN) {
			cout << getName() << "::writeFifo() - error writing to fifo" << endl;
			return -1;
		}
		//if( total < fifo_msg.size && verbose )
			//cout << getName() << "::writeFifo() - need to write more bytes " << endl;

	} while (total < fifo_msg.size);
	return total;
}

/************************************************************\
|                 ThreadedPipe::ACK                          |
\************************************************************/
void ThreadedPipe::ACK(InputType type)
{
	Client * me = shmPtr->clients.getClient(0);
	if(me == NULL) return;

	// must do this first, so not to loose the id of command we
	// are acking
	CMD_PARAM(fifo_msg.data, 0) = CMD_CMD(fifo_msg.data);

	// no finish formatting ACK
	CMD_CMD(fifo_msg.data)  = CMD_ACK;
	CMD_NUM(fifo_msg.data)  = 1;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.dest = fifo_msg.src;
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(type);
}

void ThreadedPipe::NACK(InputType type)
{
	Client * me = shmPtr->clients.getClient(0);
	if(me == NULL) return;

	// must do this first, so not to loose the id of command we
	// are acking
	CMD_PARAM(fifo_msg.data, 0) = CMD_CMD(fifo_msg.data);

	// no finish formatting NACK
	CMD_CMD(fifo_msg.data)  = CMD_FAIL;
	CMD_NUM(fifo_msg.data)  = 1;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest = fifo_msg.src;
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(type);
}

/************************************************************\
|                 ThreadedPipe::subscribe                    |
\************************************************************/
void ThreadedPipe::subscribe(uint8_t id, Client * a_client, bool onOff, InputType type)
{
	Client * me = shmPtr->clients.getClient(0);

	CMD_CMD(fifo_msg.data)  = CMD_STREAM_REQ;
	CMD_NUM(fifo_msg.data) = sizeof(StreamReq_t);
	StreamReq_t * request = (StreamReq_t*)CMD_DATAPTR(fifo_msg.data);
		
	request->id = id;
	request->client.s_addr = shmPtr->clients.getAddr(me,a_client).s_addr;
  if(request->client.s_addr == INADDR_ANY)
    request->client.s_addr = shmPtr->clients.getAddr(me).s_addr;
	request->port = 1000;
	request->send_data = onOff;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.src.s_addr = request->client.s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(type);
}

/************************************************************\
|                 ThreadedPipe::close                        |
\************************************************************/
/*!
	\brief
	\pre
	\post
 */
void ThreadedPipe::close() 
{
	for(int i=0; i<4; i++) {
		if(fifo_FDs[i] > 0){
			::close(fifo_FDs[i]);	
			remove(fifo_names[i].c_str());
      fifo_FDs[i] = -1;
		}
	}
}

/************************************************************\
|                 ThreadedPipe::update_gps_str               |
\************************************************************/
void ThreadedPipe::update_gps_str()
{
	Client * me = shmPtr->clients.getClient(0);
	//TimeStamp ts; ts.stamp();
	//sprintf(gpsStr,"(%lf, %i:%i:%f, %f, %f, %f)",
			//ts.time(),	
	sprintf(gpsStr,"%4d/%02d/%02d, %i:%i:%f, %f, %f, %f, %f, %f",
			me->telemetry.gps.date.year,
			me->telemetry.gps.date.month,
			me->telemetry.gps.date.day,
			me->telemetry.gps.utc.hours,
			me->telemetry.gps.utc.minutes,
			me->telemetry.gps.utc.seconds,
			me->telemetry.gps.pos.lat,
			me->telemetry.gps.pos.lon,
			me->telemetry.gps.pos.alt,
			me->telemetry.gps.sog,
			me->telemetry.gps.track
		);
}
